/*
 * Copyright 2018 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mapping/global_trajectory_builder.h"
#include "mapping/motion_filter.h"
#include "common/time.h"

#include <memory>

namespace cartographer {
namespace mapping {

GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(const int trajectory_id, std::shared_ptr<Publisher> publisher_ptr) : trajectory_id_(trajectory_id) {
    // TODO: 
    std::vector<std::string> expected_range_sensor_ids;
    expected_range_sensor_ids.push_back("sick_middle");
    
    publisher_ptr_ = publisher_ptr;

    local_trajectory_builder_2d_ptr_ = std::unique_ptr<LocalTrajectoryBuilder2D>(new LocalTrajectoryBuilder2D(expected_range_sensor_ids, publisher_ptr));

}
/**
  * @brief 点云数据的处理, 先进行扫描匹配, 然后将扫描匹配的结果当做节点插入到后端的位姿图中
  * 
  * @param[in] sensor_id topic名字
  * @param[in] timed_point_cloud_data 点云数据
  */
void GlobalTrajectoryBuilder::AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) {
    CHECK(local_trajectory_builder_2d_ptr_)
        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
    
    // 进行扫描匹配, 返回匹配后的结果
    std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
        matching_result = local_trajectory_builder_2d_ptr_->AddRangeData(
            sensor_id, timed_point_cloud_data);

    if (matching_result == nullptr) {
      // The range data has not been fully accumulated yet.
      return;
    }
}

// imu数据的处理, 数据走向有两个,一个是进入前端local_trajectory_builder_,一个是进入后端pose_graph_
void GlobalTrajectoryBuilder::AddSensorData(const std::string& sensor_id,
                                            const sensor::ImuData& imu_data) {
    if (local_trajectory_builder_2d_ptr_) {
      local_trajectory_builder_2d_ptr_->AddImuData(imu_data);
    }
    // pose_graph_->AddImuData(trajectory_id_, imu_data);
}

// 里程计数据的处理, 数据走向有两个,一个是进入前端local_trajectory_builder_, 一个是进入后端pose_graph_
// 加入到后端之前, 先做一个距离的计算, 只有时间,移动距离,角度 变换量大于阈值才加入到后端中
void GlobalTrajectoryBuilder::AddSensorData(const std::string& sensor_id,
                                            const sensor::OdometryData& odometry_data) {
    // CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
    if (local_trajectory_builder_2d_ptr_) {
      local_trajectory_builder_2d_ptr_->AddOdometryData(odometry_data);
    }
    
    // TODO: Instead of having an optional filter on this level,
    // odometry could be marginalized between nodes in the pose graph.
    // Related issue: cartographer-project/cartographer/#1768
    // if (pose_graph_odometry_motion_filter_.has_value() &&
    //     pose_graph_odometry_motion_filter_.value().IsSimilar(
    //         odometry_data.time, odometry_data.pose)) {
    //   return;
    // }
    // pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}

// gps数据只在后端中使用
// void AddSensorData(
  //     const std::string& sensor_id,
  //     const sensor::FixedFramePoseData& fixed_frame_pose) override {
  //   if (fixed_frame_pose.pose.has_value()) {
  //     CHECK(fixed_frame_pose.pose.value().IsValid())
  //         << fixed_frame_pose.pose.value();
  //   }
  //   pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
// }

// // Landmark的数据只在后端中使用
// void AddSensorData(const std::string& sensor_id,
  //                    const sensor::LandmarkData& landmark_data) override {
  //   pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
// }


}  // namespace mapping
}  // namespace cartographer
